#pragma once
#include "../Math/Matrix.hpp"
#include "../Math/Vector.hpp"

//Gram-Schmidt process
//to normalize and orthogonalize a set of vectors
//based on http://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process
//QR based on http://en.wikipedia.org/wiki/Qr_decomposition

namespace zzz{
template<typename T>
class GramSchmidt
{
private:
  template<int N>
  static Vector<N, T> Proj(const Vector<N, T> &u, const Vector<N, T> &v)
  {
    return u*(FastDot(u, v)/FastDot(u, u));
  }

public:
  template<int N>
  static void Process(const MatrixBase<N, N, T> &input, MatrixBase<N, N, T> &output)
  {
    Matrix<N, N, T> u;
    for (int i=0; i<N; i++)
    {
      output.Row(i)=input.Row(i);
      for (int j=0; j<i; j++)
        output.Row(i)-=Proj(output.Row(j),input.Row(i));
    }
    for (int i=0; i<N; i++)
      output.Row(i).Normalize();
  }

  //basis is column in A and Q
  template<int N>
  static void QRDecomposition(const MatrixBase<N, N, T> &input, MatrixBase<N, N, T> &Q, MatrixBase<N, N, T> &R)
  {
    Matrix<N, N, T> A(input.Transposed());
    Process(A, Q);
    R.Zero();
    for (int i=0; i<N; i++) for (int j=i; j<N; j++)
      R(i, j)=FastDot(A.Row(j), Q.Row(i));
    Q.Transpose();

  }
};
}